﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace CokAracliKontrol.VehicleControl
{
    class VehicleRoof
    {
        //const double MaximumDistance = 40;
        const double MaximumDistance = 600;
        protected Rectangle[] rects;
        protected Rectangle back1, back2, front;

        public Point backCenter, frontCenter, center;
        public double angle;

        public VehicleRoof(Rectangle rect1, Rectangle rect2, Rectangle rect3)
        {
            rects = new Rectangle[3] { rect1, rect2, rect3 };
        }

        static protected Point CenterOf(Rectangle r)
        {
            int x, y;

            x = (r.Left + r.Right) / 2;
            y = (r.Top + r.Bottom) / 2;

            return new Point(x, y);
        }

        static protected double DistanceBetween(Point p1, Point p2)
        {
            int dx, dy;

            dx = p2.X - p1.X;
            dy = p2.Y - p1.Y;

            return Math.Sqrt(dx * dx + dy * dy);
        }

        public bool Process()
        {
            // Classify rectangles as back1, back2, front
            int i, j;
            Point center1, center2;
            double d;

            double mindist = double.MaxValue;
            double maxdist = 0;
            int i_back1 = -1, i_back2 = -1, i_front = -1;
            for (i = 0; i < rects.Length; i++)
            {
                center1 = CenterOf(rects[i]);
                for (j = 0; j < rects.Length; j++)
                {
                    if (i != j)
                    {
                        center2 = CenterOf(rects[j]);

                        d = DistanceBetween(center1, center2);

                        if (d < mindist)
                        {
                            mindist = d;
                            i_back1 = i;
                            i_back2 = j;
                        }

                        if (d > maxdist) maxdist = d;
                    }
                }
            }

            if (maxdist > MaximumDistance)
            {
                return false;
            }

            for (i = 0; i < rects.Length; i++)
            {
                if (i != i_back1 && i != i_back2)
                {
                    i_front = i;
                    break;
                }
            }

            back1 = rects[i_back1];
            back2 = rects[i_back2];
            front = rects[i_front];

            // calculate back's center
            Point p1, p2;

            p1 = CenterOf(back1);
            p2 = CenterOf(back2);
            backCenter = new Point((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2);

            // calculate vehicle's center
            p1 = backCenter;
            frontCenter = CenterOf(front);
            p2 = frontCenter;
            center = new Point((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2);

            // calculate the angle
            double dx, dy;
            dx = Math.Abs(frontCenter.X - backCenter.X);
            dy = Math.Abs(frontCenter.Y - backCenter.Y);

            if (backCenter.X < frontCenter.X)
            {
                if (backCenter.Y > frontCenter.Y)
                {
                    angle = Math.Atan(dx / dy);
                }
                else
                {
                    angle = Math.Atan(dy / dx) + Math.PI / 2;
                }
            }
            else
            {
                if (backCenter.Y < frontCenter.Y)
                {
                    angle = Math.Atan(dx / dy) + Math.PI;
                }
                else
                {
                    angle = -Math.Atan(dx / dy) + 2 * Math.PI;
                }
            }

            return true;
        }

        public double AngleAsDegrees
        {
            get
            {
                return (angle / Math.PI) * 180;
            }
        }
    }
}
